# -*- coding: utf-8 -*-
# Filename: ins_data_manager.py

"""
Manage all possible generated in an INS solution.
Created on 2018-04-24
@author: dongxiaoguang
"""

import math
import numpy as np
from . import sim_data
from .sim_data import Sim_data
from ..attitude import attitude
from ..kml_gen import kml_gen
from ..geoparams import geoparams

class InsDataMgr(object):
    '''
    A class that manage all data generated in an INS solution. For example, reference data,
    sensor data, algorithm results. These data can be saved to files or plot in figures.
    '''
    def __init__(self, fs, ref_frame=0):
        '''
        Args:
            fs: [fs_imu, fs_gps, fs_mag], Hz.
                fs_imu: The sample rate of IMU.
                fs_gps: The sample rate of GPS.
                fs_mag: not used now. The sample rate of the magnetometer is
                    the same as that of the imu.

            ref_frame: reference frame used as the navigation frame,
                        0: NED (default), with x axis pointing along geographic north,
                            y axis pointing eastward,
                            z axis pointing downward.
                        1: a virtual inertial frame with constant g,
                            x axis pointing along magnetic north,
                            z axis pointing along g,
                            y axis completing a right-handed coordinate system.
                            Notice: For this virtual inertial frame, position is indeed the sum of
                            the initial position in ecef and the relative position in the virutal
                            inertial frame.
        '''
        # sample rate
        self.fs = Sim_data(name='fs',\
                           description='Sample frequency of IMU',\
                           units=['Hz'],\
                           plottable=False)
        self.fs_gps = Sim_data(name='fs_gps',\
                            description='Sample frequency of GPS',\
                            units=['Hz'],\
                            plottable=False)
        self.fs_mag = Sim_data(name='fs_mag',\
                            description='Sample frequency of Magnetometer',\
                            units=['Hz'],\
                            plottable=False)
        # reference frame
        self.ref_frame = Sim_data(name='ref_frame',\
                                    description='Reference frame',\
                                    plottable=False)
        if ref_frame == 0 or ref_frame == 1:
            self.ref_frame.data = ref_frame
        else:
            self.ref_frame.data = 0      # default frame is NED

        ########## possible data generated by simulation ##########
        # reference data
        self.time = Sim_data(name='time',\
                             description='sample time',\
                             units=['sec'],\
                             legend=['time'])
        self.gps_time = Sim_data(name='gps_time',\
                                 description='GPS sample time',\
                                 units=['sec'],\
                                 legend=['gps_time'])
        self.gps_visibility = Sim_data(name='gps_visibility',\
                                       description='GPS visibility',\
                                       legend=['gps_visibility'])
        self.ref_pos = Sim_data(name='ref_pos',\
                                description='true LLA pos in the navigation frame',\
                                units=['rad', 'rad', 'm'],\
                                output_units=['deg', 'deg', 'm'],\
                                legend=['ref_pos_lat', 'ref_pos_lon', 'ref_pos_alt'])
        self.ref_vel = Sim_data(name='ref_vel',\
                                description='true vel in the NED frame',\
                                units=['m/s', 'm/s', 'm/s'],\
                                legend=['ref_vel_x', 'ref_vel_y', 'ref_vel_z'])
        self.ref_att_euler = Sim_data(name='ref_att_euler',\
                                description='true attitude (Euler angles, ZYX)',\
                                units=['rad', 'rad', 'rad'],\
                                output_units=['deg', 'deg', 'deg'],\
                                legend=['ref_Yaw', 'ref_Pitch', 'ref_Roll'])
        self.ref_att_quat = Sim_data(name='ref_att_quat',\
                                     description='true attitude (quaternion)',\
                                     legend=['q0', 'q1', 'q2', 'q3'])
        self.ref_gyro = Sim_data(name='ref_gyro',\
                                 description='true angular velocity in the body frame',\
                                 units=['rad/s', 'rad/s', 'rad/s'],\
                                 output_units=['deg/s', 'deg/s', 'deg/s'],\
                                 legend=['ref_gyro_x', 'ref_gyro_y', 'ref_gyro_z'])
        self.ref_accel = Sim_data(name='ref_accel',\
                                  description='true accel in the body frame',\
                                  units=['m/s^2', 'm/s^2', 'm/s^2'],\
                                  legend=['ref_accel_x', 'ref_accel_y', 'ref_accel_z'])
        self.ref_gps = Sim_data(name='ref_gps',\
                                description='true GPS LLA position and NED velocity',\
                                units=['rad', 'rad', 'm', 'm/s', 'm/s', 'm/s'],\
                                output_units=['deg', 'deg', 'm', 'm/s', 'm/s', 'm/s'],\
                                legend=['ref_gps_lat', 'ref_gps_lon', 'ref_gps_alt',\
                                        'ref_gps_vN', 'ref_gps_vE', 'ref_gps_vD'])
                                # downsampled true pos/vel
        self.ref_odo = Sim_data(name='ref_odo',\
                                description='true odometer velocity',\
                                units=['m/s'],\
                                legend=['ref_odo'])
        self.ref_mag = Sim_data(name='ref_mag',\
                                description='true magnetic field in the body frame',\
                                units=['uT', 'uT', 'uT'],\
                                legend=['ref_mag_x', 'ref_mag_y', 'ref_mag_z'])
        # sensor measurements
        self.gyro = Sim_data(name='gyro',\
                             description='gyro measurements',\
                             units=['rad/s', 'rad/s', 'rad/s'],\
                             output_units=['deg/s', 'deg/s', 'deg/s'],\
                             legend=['gyro_x', 'gyro_y', 'gyro_z'])
        self.accel = Sim_data(name='accel',\
                              description='accel measurements',\
                              units=['m/s^2', 'm/s^2', 'm/s^2'],\
                              legend=['accel_x', 'accel_y', 'accel_z'])
        self.gps = Sim_data(name='gps',\
                            description='GPS LLA position and NED velocity measurements',\
                            units=['rad', 'rad', 'm', 'm/s', 'm/s', 'm/s'],\
                            output_units=['deg', 'deg', 'm', 'm/s', 'm/s', 'm/s'],\
                            legend=['gps_lat', 'gps_lon', 'gps_alt',\
                                    'gps_vN', 'gps_vE', 'gps_vD'])
        self.odo = Sim_data(name='odo',\
                            description='odometer velocity measurement',\
                            units=['m/s'],\
                            legend=['odo'])
        self.mag = Sim_data(name='mag',\
                            description='magnetometer measurements',\
                            units=['uT', 'uT', 'uT'],\
                            legend=['mag_x', 'mag_y', 'mag_z'])
        # calibration algorithm output
        self.gyro_cal = Sim_data(name='gyro_cal',\
                                 description='gyro measurements after factory calibration',\
                                 units=['rad/s', 'rad/s', 'rad/s'],\
                                 output_units=['deg/s', 'deg/s', 'deg/s'],\
                                 legend=['gyro_x', 'gyro_y', 'gyro_z'])
        self.accel_cal = Sim_data(name='accel_cal',\
                                  description='accel measurements after factory calibration',\
                                  units=['m/s^2', 'm/s^2', 'm/s^2'],\
                                  legend=['accel_x', 'accel_y', 'accel_z'])
        self.mag_cal = Sim_data(name='mag_cal',\
                                description='magnetometer measurements after SI&HI calibration',\
                                units=['uT', 'uT', 'uT'],\
                                legend=['mag_x', 'mag_y', 'mag_z'])
        self.soft_iron = Sim_data(name='soft_iron',\
                                description='soft iron calibration matrix',\
                                plottable=False)
        self.hard_iron = Sim_data(name='hard_iron',\
                                description='hard iron',\
                                units=['uT', 'uT', 'uT', 'uT'],\
                                legend=['offset_x', 'offset_y', 'offset_z', 'radius'],\
                                plottable=False)
        # fusion algorithm output
        self.algo_time = Sim_data(name='algo_time',\
                                  description='sample time from algo',\
                                  units=['sec'])
        self.pos = Sim_data(name='pos',\
                            description='simulation position from algo',\
                            units=['rad', 'rad', 'm'],\
                            output_units=['deg', 'deg', 'm'],\
                            legend=['pos_lat', 'pos_lon', 'pos_alt'])
        self.vel = Sim_data(name='vel',\
                            description='simulation velocity from algo',\
                            units=['m/s', 'm/s', 'm/s'],\
                            legend=['vel_x', 'vel_y', 'vel_z'])
        self.att_quat = Sim_data(name='att_quat',\
                                 description='simulation attitude (quaternion)  from algo',\
                                 legend=['q0', 'q1', 'q2', 'q3'])
        self.att_euler = Sim_data(name='att_euler',
                                  description='simulation attitude (Euler, ZYX)  from algo',\
                                  units=['rad', 'rad', 'rad'],\
                                  output_units=['deg', 'deg', 'deg'],\
                                  legend=['Yaw', 'Pitch', 'Roll'])
        self.wb = Sim_data(name='wb',\
                           description='gyro bias estimation',\
                           units=['rad/s', 'rad/s', 'rad/s'],\
                           output_units=['deg/s', 'deg/s', 'deg/s'],\
                           legend=['gyro_bias_x', 'gyro_bias_y', 'gyro_bias_z'])
        self.ab = Sim_data(name='ab',\
                           description='accel bias estimation',\
                           units=['m/s^2', 'm/s^2', 'm/s^2'],\
                           legend=['accel_bias_x', 'accel_bias_y', 'accel_bias_z'])
        self.ad_gyro = Sim_data(name='ad_gyro',\
                                description='Allan deviation of gyro',\
                                units=['rad/s', 'rad/s', 'rad/s'],\
                                output_units=['deg/s', 'deg/s', 'deg/s'],\
                                logx=True, logy=True,\
                                legend=['AD_wx', 'AD_wy', 'AD_wz'])
        self.ad_accel = Sim_data(name='ad_accel',\
                                 description='Allan deviation of accel',\
                                 units=['m/s^2', 'm/s^2', 'm/s^2'],\
                                 logx=True, logy=True,\
                                 legend=['AD_ax', 'AD_ay', 'AD_az'])
        # if using virtual inertial frame
        if self.ref_frame.data == 1:
            # description, position units and legned
            self.ref_pos.description = 'true position in the local NED frame'
            self.ref_pos.units = ['m', 'm', 'm']
            self.ref_pos.output_units = ['m', 'm', 'm']
            self.ref_pos.legend = ['ref_pos_x', 'ref_pos_y', 'ref_pos_z']
            self.pos.units = ['m', 'm', 'm']
            self.pos.output_units = ['m', 'm', 'm']
            self.pos.legend = ['pos_x', 'pos_y', 'pos_z']
            # velocity units and legend
            self.ref_vel.legend = ['ref_vel_x', 'ref_vel_y', 'ref_vel_z']
            self.vel.legend = ['vel_x', 'vel_y', 'vel_z']
            # GPS units and legend
            self.ref_gps.description = 'true GPS position and velocity in the local NED frame'
            self.ref_gps.units = ['m', 'm', 'm', 'm/s', 'm/s', 'm/s']
            self.ref_gps.output_units = ['m', 'm', 'm', 'm/s', 'm/s', 'm/s']
            self.ref_gps.legend = ['ref_gps_x', 'ref_gps_y', 'ref_gps_z',\
                                   'ref_gps_vx', 'ref_gps_vy', 'ref_gps_vz']
            self.gps.description = 'GPS position and velocity measurements in the local NED frame'
            self.gps.units = ['m', 'm', 'm', 'm/s', 'm/s', 'm/s']
            self.gps.output_units = ['m', 'm', 'm', 'm/s', 'm/s', 'm/s']
            self.gps.legend = ['gps_x', 'gps_y', 'gps_z',\
                               'gps_vx', 'gps_vy', 'gps_vz']
        ########## all data ##########
        # __all include all data that may occur in an INS solution.
        self.__all = {
            # error-free data
            self.fs.name: self.fs,
            self.fs_gps.name: self.fs_gps,
            self.fs_mag.name: self.fs_mag,
            self.ref_frame.name: self.ref_frame,
            self.time.name: self.time,
            self.gps_time.name: self.gps_time,
            self.gps_visibility.name: self.gps_visibility,
            self.ref_pos.name: self.ref_pos,
            self.ref_vel.name: self.ref_vel,
            self.ref_att_euler.name: self.ref_att_euler,
            self.ref_att_quat.name: self.ref_att_quat,
            self.ref_gyro.name: self.ref_gyro,
            self.ref_accel.name: self.ref_accel,
            self.ref_gps.name: self.ref_gps,
            self.ref_odo.name: self.ref_odo,
            self.ref_mag.name: self.ref_mag,
            # sensor data
            self.gyro.name: self.gyro,
            self.accel.name: self.accel,
            self.gps.name: self.gps,
            self.odo.name: self.odo,
            self.mag.name: self.mag,
            # calibration algorithm output
            self.gyro_cal.name: self.gyro_cal,
            self.accel_cal.name: self.accel_cal,
            self.mag_cal.name: self.mag_cal,
            self.soft_iron.name: self.soft_iron,
            self.hard_iron.name: self.hard_iron,
            # fusion algorithm output
            self.algo_time.name: self.algo_time,
            self.pos.name: self.pos,
            self.vel.name: self.vel,
            self.att_quat.name: self.att_quat,
            self.att_euler.name: self.att_euler,
            self.wb.name: self.wb,
            self.ab.name: self.ab,
            self.ad_gyro.name: self.ad_gyro,
            self.ad_accel.name: self.ad_accel
            }
        # all available data that really occur in the simulation.
        self.available = []
        self.available.append(self.ref_frame.name)
        if fs[0] is not None:
            self.fs.data = fs[0]
            self.available.append(self.fs.name)
        else:
            raise ValueError('IMU sampling frequency cannot be None.')
        if fs[1] is not None:
            self.fs_gps.data = fs[1]
            self.available.append(self.fs_gps.name)
        if fs[2] is not None:
            self.fs_mag.data = fs[2]
            self.available.append(self.fs_mag.name)
        # the following will not be saved
        self.__do_not_save = [self.fs.name, self.fs_gps.name,\
                            self.fs_mag.name, self.ref_frame.name]
        # algorithm output
        self.__algo_output = []
        # associated data mapping. If the user want Euler angles, but only quaternions are
        # available, Euler angles will be automatically calculated, added to self.available and
        # returned.
        self.__data_map = {self.ref_att_euler.name: [self.ref_att_quat, self.__euler2quat_zyx],
                           self.ref_att_quat.name: [self.ref_att_euler, self.__quat2euler_zyx],
                           self.att_euler.name: [self.att_quat, self.__euler2quat_zyx],
                           self.att_quat.name: [self.att_euler, self.__quat2euler_zyx]}
        # error info, self.get_error_stat() and self.plot() will both update error info
        self.__err = {}

    def add_data(self, data_name, data, key=None, units=None):
        '''
        Add data to available.
        Args:
            data_name: data name
            data: a scalar, a numpy array or a dict of the above two. If data is a dict, each
                value in it should be of same type (scalar or numpy array), same size and same
                units.
            key: There are more than one set of data, key is an index of data added this time.
                If key is None, data can be a scalr, a numpy array or a dict of the above two.
                If key is a valid dict key, data can be a scalr or a numpy.
            units: Units of the data. If you know clearly no units convertion is needed, set
                units to None. If you do not know what units are used in the class InsDataMgr,
                you'd better provide the units of the data. Units convertion will be done
                automatically.
                If data is a scalar, units should be a list of one string to define its unit.
                If data is a numpy of size(m,n), units should be a list of n strings
                to define the units.
                If data is a dict, units should be the same as the above two depending on if
                each value in the dict is a scalar or a numpy array.
        '''
        if data_name in self.__all:
            self.__all[data_name].add_data(data, key, units)
            if data_name not in self.available:
                self.available.append(data_name)
        else:
            raise ValueError("Unsupported data: %s."%data_name)

    def set_algo_output(self, algo_output):
        '''
        Tell data manager what output an algorithm provide
        Args:
            algo_output: a list of data names.
        '''
        for i in algo_output:
            if self.is_supported(i):
                self.__algo_output.append(i)
            else:
                raise ValueError("Unsupported algorithm output: %s."% i)

    def get_data(self, data_names):
        '''
        Get data section of data_names.
        Args:
            data_names: a list of data names
        Returns:
            data: a list of data corresponding to data_names.
            If there is any unavailable data in data_names, return None
        '''
        data = []
        for i in data_names:
            if i in self.available:
                data.append(self.__all[i].data)
            else:
                print('%s is not available.'% i)
                return None
        return data

    def get_data_all(self, data_name):
        '''
        get the Sim_data object accroding to data_name
        '''
        if data_name in self.__all:
            return self.__all[data_name]
        else:
            return None

    def get_data_properties(self, data_name):
        '''
        Get the properties of the data specified by data_name.
        Args:
            data_name: a string to specify the data
        Returns:
            [description, units, plottable, logx, logy, legend]
        '''
        return [self.__all[data_name].description,\
                self.__all[data_name].units,\
                self.__all[data_name].plottable,\
                self.__all[data_name].logx,\
                self.__all[data_name].logy,\
                self.__all[data_name].legend]

    def get_error_stats(self, data_name, err_stats_start=0, angle=False,\
                       use_output_units=False, extra_opt=''):
        '''
        Get error statistics of data_name.
        Args:
            data_name: name of data whose error will be calculated.
            err_stats_start: This argument specify the starting point in seconds from where
                the error statistics are calculated.
                If it is -1, end-point error statistics will be calculated. In this case,
                the result contains statistics of end-point errors of multiple runs.
                Otherwise, the process error of data specified by data_name starting from
                err_stats_start(in seconds) is calculated. In this case, the result is
                a dict of statistics of process error of each simulatoin run.
                For example, if we want the end-point error of position from a free-integration
                algorithm ran for n times, the result is {'max': numpy.array([rx, ry, rz]),
                                                          'avg': numpy.array([rx, ry, rz]),
                                                          'std': numpy.array([rx, ry, rz])}.
                If we want the process error of an attitude determination algorithm ran for n
                times, the result is {'max': a dict of numpy.array([yaw, pitch, roll]),
                                      'avg': a dict of numpy.array([yaw, pitch, roll]),
                                      'std': a dict of numpy.array([yaw, pitch, roll])}.
            angle: True if this is angle error. Angle error will be converted to be within
                [-pi, pi] before calculating statistics.
            use_output_units: use output units instead of inner units in which the data are
                stored. An automatic unit conversion is done.
            extra_opt: A string option to calculate errors. The following options are supported:
                'ned': NED position error
        Returns:
            err_stat: error statistics.
        '''
        err_stat = None
        # is this set of data available?
        if data_name not in self.available:
            print('error stats: %s is not available.'% data_name)
            return None
        # is the reference of data_name available?
        ref_data_name = 'ref_' + data_name
        if ref_data_name not in self.available:
            print('%s has no reference.'% data_name)
            return None
        # calculate error
        err_data_name = 'err_' + data_name
        if err_data_name not in self.__err:
            data_err = self.calc_data_err(data_name, ref_data_name, angle, extra_opt)
            if data_err is not None:
                self.__err[data_err.name] = data_err
        if err_stats_start == -1:
            # end-point error
            err_stat = self.__end_point_error_stats(data_name)
        else:
            # process error
            err_stat = self.__process_error_stats(data_name, err_stats_start)
        # unit conversion
        if use_output_units and (err_stat is not None):
            for i in err_stat:
                if isinstance(err_stat[i], dict):
                    for j in err_stat[i]:
                        err_stat[i][j] = \
                            sim_data.convert_unit(err_stat[i][j],\
                                                  self.__err[err_data_name].units,\
                                                  self.__err[err_data_name].output_units)
                else:
                    err_stat[i] = \
                            sim_data.convert_unit(err_stat[i],\
                                                  self.__err[err_data_name].units,\
                                                  self.__err[err_data_name].output_units)
        err_stat['units'] = str(self.__err[err_data_name].output_units)
        return err_stat

    def calc_data_err(self, data_name, ref_data_name, angle=False, err_opt=''):
        '''
        Calculate error of one set of data.
        Args:
            data_name: name of the intput data
            ref_data_name: name of the reference of the intput data
            angle: True if this is angle error. Angle error will be converted to be within
                [-pi, pi] before calculating statistics.
            err_opt: error options
        Returns:
            an Sim_data object corresponds to data_name
        '''
        err = Sim_data(name='err_'+data_name,\
                       description='ERROR of '+self.__all[data_name].description,\
                       units=self.__all[data_name].units,\
                       output_units=self.__all[data_name].output_units,\
                       plottable=self.__all[data_name].plottable,\
                       logx=self.__all[data_name].logx, logy=self.__all[data_name].logy,\
                       grid=self.__all[data_name].grid,\
                       legend=self.__all[data_name].legend)
        # handling position error
        lla = 0
        if data_name == self.pos.name and self.ref_frame.data == 0:
            if err_opt == 'ned':
                lla = 1
                err.description = 'ERROR of NED position'
                err.units = ['m', 'm', 'm']
                err.output_units = ['m', 'm', 'm']
                err.legend = ['pos_N', 'pos_E', 'pos_D']
            elif err_opt == 'ecef':
                lla = 2
                err.description = 'ERROR of ECEF position'
                err.units = ['m', 'm', 'm']
                err.output_units = ['m', 'm', 'm']
                err.legend = ['pos_x', 'pos_y', 'pos_z']
        if isinstance(self.__all[data_name].data, dict):
            ref_data = None
            for i in self.__all[data_name].data:
                # get raw reference data for first key in the dict, use reference from last
                # step for other keys to avoid multiple interps.
                if ref_data is None:
                    # use copy to avoid changing data if interp
                    ref_data = self.__all[ref_data_name].data.copy()
                # Interpolation. using ref_data to avoid multiple interps
                if ref_data.shape[0] != self.__all[data_name].data[i].shape[0]:
                    # print("%s has different number of samples from its reference."% data_name)
                    # print('Interpolation needed.')
                    if self.algo_time.name in self.available and self.time.name in self.available:
                        ref_data = self.__interp(self.algo_time.data[i],\
                                                 self.time.data, self.__all[ref_data_name].data)
                    else:
                        print("%s or %s is not available."% (self.algo_time.name, self.time.name))
                        return None
                # error stat
                err.data[i] = self.array_error(self.__all[data_name].data[i], ref_data, angle, lla)
        elif isinstance(self.__all[data_name].data, np.ndarray):
            ref_data = self.__all[ref_data_name].data.copy()
            # Interpolation
            if ref_data.shape[0] != self.__all[data_name].data.shape[0]:
                print("%s has different number of samples from its reference."% data_name)
                print('Interpolation needed.')
                if self.algo_time.name in self.available and self.time.name in self.available:
                    ref_data = self.__interp(self.algo_time.data, self.time.data, ref_data)
                else:
                    print("%s or %s is not available."% (self.algo_time.name, self.time.name))
                    return None
            # error stat
            err.data = self.array_error(self.__all[data_name].data, ref_data, angle, lla)
        return err

    def array_error(self, x, r, angle=False, lla=0):
        '''
        Calculate the error of an array w.r.t its reference.
        Args:
            x: input data, numpy array.
            r: reference data, same size as x.
            angle: True if x contains angles, False if not.
            pos: 0 if x is not in LLA form;
                 1 if x is in LLA form, and NED error is required;
                 >=2 if x is in LLA form, and ECEF error is required.
        Returns:
            err: error
        '''
        if lla == 0:
            err = x - r
            if angle:
                for j in range(len(err.flat)):
                    err.flat[j] = attitude.angle_range_pi(err.flat[j])
        else:
            # convert x and r to ECEF first
            x_ecef = geoparams.lla2ecef_batch(x)
            r_ecef = geoparams.lla2ecef_batch(r)
            err = x_ecef - r_ecef
            # convert ecef err to NED err
            if lla == 1:
                n = err.shape[0]
                for i in range(0, n):
                    c_ne = attitude.ecef_to_ned(r[i, 0], r[i, 1])
                    err[i, :] = c_ne.dot(err[i, :])
        return err

    def save_data(self, data_dir):
        '''
        save data to files
        Args:
            data_dir: Data files will be saved in data_idr
        Returns:
            data_saved: a list of data that are saved.
        '''
        data_saved = []
        for data in self.available:
            if data not in self.__do_not_save:
                # print('saving %s'% data)
                self.__all[data].save_to_file(data_dir)
                data_saved.append(data)
        return data_saved

    def plot(self, what_to_plot, keys, angle=False, opt=None, extra_opt=''):
        '''
        Plot specified results.
        Args:
            what_to_plot: a string to specify what to plot. See manual for details.
            keys: specify the simulation data of multiple run. This can be an integer, or a list
                or tuple. Each element should be within [0, num_times-1]. Default is None, and
                plot data of all simulations.
            opt: a dict to specify plot options. its keys are composed of elements in what_to_plot.
                values can be:
                    'error': plot the error of the data specified by what_to_plot w.r.t ref
                    '3d': 3d plot
            extra_opt: strings to specify matplotlib properties.
        '''
        if what_to_plot in self.available:
            # get plot options
            ref_data_name = None
            plot3d = 0
            # this data has plot options?
            if isinstance(opt, dict):
                if what_to_plot in opt:
                    if opt[what_to_plot].lower() == '3d':
                        plot3d = 1
                    elif opt[what_to_plot].lower() == 'projection':
                        plot3d = 2
                    elif opt[what_to_plot].lower() == 'error':
                        # this data have reference, error can be calculated
                        ref_data_name = 'ref_' + what_to_plot
                        if ref_data_name not in self.available:
                            ref_data_name = None
                            print(what_to_plot + ' has no reference.')
            # default x axis data
            x_axis = self.time
            # choose proper x axis data for specific y axis data
            if what_to_plot == self.ref_gps.name or what_to_plot == self.gps.name or\
                what_to_plot == self.gps_visibility.name or what_to_plot == self.gps_time.name:
                x_axis = self.gps_time
            elif what_to_plot in self.__algo_output and self.algo_time.name in self.available:
                x_axis = self.algo_time
            # plot
            if ref_data_name is not None:
                err_data_name = 'err_' + what_to_plot
                # error data not generated yet, generate it
                if err_data_name not in self.__err:
                    data_err = self.calc_data_err(what_to_plot, ref_data_name, angle=angle)
                    if data_err is not None:
                        self.__err[data_err.name] = data_err
                # error data generated, plot it
                if err_data_name in self.__err:
                    self.__err[err_data_name].plot(x_axis, key=keys,\
                                                   plot3d=plot3d, mpl_opt=extra_opt)
                else:
                    print('Cannot get error data of %s'% what_to_plot)
            else:
                self.__all[what_to_plot].plot(x_axis, key=keys,\
                                              plot3d=plot3d, mpl_opt=extra_opt)
        else:
            print('Unsupported plot: %s.'% what_to_plot)
            # print("Only the following data are available for plot:")
            # print(list(self.supported_plot.keys()))
            # raise ValueError("Unsupported data to plot: %s."%data)

    def show_plot(self):
        '''
        Show all plots
        '''
        sim_data.show_plot()

    def save_kml_files(self, data_dir):
        '''
        generate kml files from reference position and simulation position.
        Args:
            data_dir: kml files are saved in data_dir
        '''
        convert_xyz_to_lla = False
        if self.ref_frame.data == 1:
            convert_xyz_to_lla = True
        # ref position
        if 'ref_pos' in self.available:
            if 'ref_att_euler' in self.available:
                heading = self.__all['ref_att_euler'].data[:, 0] * attitude.R2D
            else:
                heading = None
            max_points = self.__all['ref_pos'].data.shape[0] / self.__all['fs'].data
            kml_gen.kml_gen(data_dir,\
                            self.__all['ref_pos'].data,\
                            heading=heading,\
                            name='ref_pos',\
                            convert_to_lla=convert_xyz_to_lla,\
                            color='ff0000ff',\
                            max_points=max_points)
        # gps position
        if 'gps' in self.available:
            for i in self.__all['gps'].data.keys():
                pos_name = 'gps_' + str(i)
                heading = np.zeros((self.__all['gps'].data[i].shape[0],))
                for j in range(self.__all['gps'].data[i].shape[0]):
                    heading[j] = math.atan2(self.__all['gps'].data[i][j,4],
                                            self.__all['gps'].data[i][j,3]) * attitude.R2D
                    self.__all['gps'].data[i][j, 0:3] *= self.__all['gps_visibility'].data[j]
                max_points = self.__all['gps'].data[i].shape[0] / self.__all['fs_gps'].data
                kml_gen.kml_gen(data_dir,\
                                self.__all['gps'].data[i][:, 0:3],\
                                heading=heading,\
                                convert_to_lla=convert_xyz_to_lla,\
                                name=pos_name,\
                                color='ff00ff00',\
                                max_points=max_points)
        # simulation position
        if 'pos' in self.available:
            for i in self.__all['pos'].data.keys():
                pos_name = 'pos_' + str(i)
                if 'att_euler' in self.available:
                    heading = self.__all['att_euler'].data[i][:, 0] * attitude.R2D
                else:
                    heading = None
                max_points = self.__all['pos'].data[i].shape[0] / self.__all['fs'].data
                kml_gen.kml_gen(data_dir,\
                                self.__all['pos'].data[i],\
                                heading=heading,\
                                convert_to_lla=convert_xyz_to_lla,\
                                name=pos_name,\
                                color='ffff0000',\
                                max_points=max_points)

    def is_supported(self, data_name):
        '''
        Tell if this set of data is supported or not
        '''
        return data_name in self.__all.keys()

    def is_available(self, data_name, key=None):
        '''
        Tell if data_name is available. If key is not None, further tell if data in data_name
        has the key.
        '''
        # if data_name is available
        rtn = data_name in self.available
        # if data_name has key
        if rtn and key is not None:
            if isinstance(self.__all[data_name].data, dict):
                rtn = key in self.__all[data_name].data.keys()
            else:
                rtn = False
        return rtn

    def __end_point_error_stats(self, data_name, group=True):
        '''
        end-point error statistics
        '''
        # error available?
        err_data_name = 'err_' + data_name
        if err_data_name not in self.__err:
            print('__end_point_error_stats: %s is not available.'% data_name)
        # collect data according to keys
        if isinstance(self.__err[err_data_name].data, dict):
            # collect groups
            groups = None
            if group:
                keys = self.__err[err_data_name].data.keys()
                groups = self.__get_data_groups(keys)
            # only one group
            if groups is None:
                # a dict contains data of multiple runs
                err = []
                for i in self.__err[err_data_name].data:
                    err.append(self.__err[err_data_name].data[i][-1, :])
                # convert list to np.array
                err = np.array(err)
                return self.__array_stats(err)
            # at least two groups
            else:
                stat = {'max': {}, 'avg': {}, 'std': {}}
                for j in groups:
                    err = []
                    for i in self.__err[err_data_name].data:
                        if j in i:
                            err.append(self.__err[err_data_name].data[i][-1, :])
                    tmp = self.__array_stats(err)
                    stat['max'][j] = tmp['max']
                    stat['avg'][j] = tmp['avg']
                    stat['std'][j] = tmp['std']
                return stat
        elif isinstance(self.__err[err_data_name].data, np.ndarray):
            err = self.__err[err_data_name].data[-1, :]
            return self.__array_stats(err)
        else:
            print('Unsupported data type to calculate error statitics for %s'% data_name)
            return None

    def __process_error_stats(self, data_name, err_stats_start):
        '''
        process error statistics
        '''
        # error available?
        err_data_name = 'err_' + data_name
        if err_data_name not in self.__err:
            print('__process_error_stats: %s is not available.'% data_name)
        # begin to calculate error stat
        if isinstance(self.__all[data_name].data, dict):
            stat = {'max': {}, 'avg': {}, 'std': {}}
            for i in self.__all[data_name].data:
                # error stat
                if i in self.algo_time.data:
                    idx = np.where(self.algo_time.data[i] >= err_stats_start)[0]
                else:
                    idx = np.where(self.time.data >= err_stats_start)[0]
                if idx.shape[0] == 0:
                    print('err_stats_start exceeds max data points.')
                    idx = 0
                else:
                    idx = idx[0]
                err = self.__err[err_data_name].data[i][idx::, :]
                tmp = self.__array_stats(err)
                stat['max'][i] = tmp['max']
                stat['avg'][i] = tmp['avg']
                stat['std'][i] = tmp['std']
            return stat
        elif isinstance(self.__all[data_name].data, np.ndarray):
            # error stat
            err = self.__err[err_data_name].data
            return self.__array_stats(err)
        else:
            print('Unsupported data type to calculate error statitics for %s'% data_name)
            return None

    def __array_stats(self, x):
        '''
        statistics of array x.
        Args:
            x is a numpy array of size (m,n) or (m,). m is number of sample. n is its dimension.
        Returns:
            {'max':, 'avg':, 'std': }
        '''
        # statistics
        return {'max': np.max(np.abs(x), 0),\
                'avg': np.average(x, 0),\
                'std': np.std(x, 0)}

    def __get_data_groups(self, keys):
        '''
        Check if the keys can be grouped. The key should be named as GROUP_idx.
        For example list of keys [algo0_0, algo0_1 algo1_0, algo1_1] can be divided
        into two groups: [algo0, algo1], and each group contains two elements. 
        This is used to calculate statistics of error of results from multiple algorithms.
        Args:
            keys: dict keys
        Return:
            a list of groups if there is more than one group, and none if there is only one group
        '''
        groups = []
        for i in keys:
            idx = str(i).rfind('_')
            if idx == -1:
                groups = [] # one of the keys cannot be grouped, do not group
                break
            group_name = i[0:idx]
            if group_name not in groups:
                groups.append(group_name)
        if len(groups) <= 1:
            groups = None
        return groups

    def __interp(self, x, xp, fp):
        '''
        data interpolation
        '''
        m = x.shape[0]
        ndim = fp.ndim
        if ndim == 1:
            return np.interp(x, xp, fp)
        elif ndim == 2:
            y = np.zeros((m, fp.shape[1]))
            for i in range(fp.shape[1]):
                y[:, i] = np.interp(x, xp, fp[:, i])
            return y
        else:
            raise ValueError('only 1-D or 2-D fp is supported.')

    def __quat2euler_zyx(self, src, dst):
        '''
        quaternion to Euler angles (zyx)
        '''
        if isinstance(src.data, np.ndarray):
            n = src.data.shape[0]
            dst.data = np.zeros((n, 3))
            for j in range(n):
                dst.data[j, :] = attitude.quat2euler(src.data[j, :])
        elif isinstance(src.data, dict):
            for i in src.data:
                n = src.data[i].shape[0]
                euler = np.zeros((n, 3))
                for j in range(n):
                    euler[j, :] = attitude.quat2euler(src.data[i][j, :])
                dst.data[i] = euler
        else:
            raise ValueError('%s is not a dict or numpy array.'% src.name)

    def __euler2quat_zyx(self, src, dst):
        '''
        Euler angles (zyx) to quaternion
        '''
        # array
        if isinstance(src.data, np.ndarray):
            n = src.data.shape[0]
            dst.data = np.zeros((n, 4))
            for j in range(n):
                dst.data[j, :] = attitude.euler2quat(src.data[j, :])
        # dict
        elif isinstance(src.data, dict):
            for i in src.data:
                n = src.data[i].shape[0]
                quat = np.zeros((n, 4))
                for j in range(n):
                    quat[j, :] = attitude.euler2quat(src.data[i][j, :])
                dst.data[i] = quat
        else:
            raise ValueError('%s is not a dict or numpy array.'% src.name)
